import { Detector } from "./Detector";
import { DetectorResult } from "../DetectorResult";
import { Geometry } from "../../geometry/Geometry";
import { Transform } from "../../../common/Transform";
import { CachedDetectorData } from "./CachedDetectorData";
import { Vec3 } from "../../../common/Vec3";
import { M } from "../../../common/M";
import { MathUtil } from "../../../common/MathUtil";
import { Setting } from "../../../common/Setting";
import { BoxGeometry } from "../../geometry/BoxGeometry";
import { BoxBoxDetectorMacro } from "./BoxBoxDetectorMacro";

/**
 * Box vs Box detector.
 */
export class BoxBoxDetector extends Detector {

	clipper: FaceClipper;

	private mId: number;

	private w1: number;
	private w2: number;
	private h1: number;
	private h2: number;
	private d1: number;
	private d2: number;

	private pos1: Vec3 = new Vec3();
	private pos2: Vec3 = new Vec3();
	private pos1_2: Vec3 = new Vec3();

	private x1: Vec3 = new Vec3();
	private y1: Vec3 = new Vec3();
	private z1: Vec3 = new Vec3();
	private x2: Vec3 = new Vec3();
	private y2: Vec3 = new Vec3();
	private z2: Vec3 = new Vec3();

	private sx1: Vec3 = new Vec3();
	private sy1: Vec3 = new Vec3();
	private sz1: Vec3 = new Vec3();
	private sx2: Vec3 = new Vec3();
	private sy2: Vec3 = new Vec3();
	private sz2: Vec3 = new Vec3();

	private proj1: number;
	private proj2: number;
	private projSum: number;
	private projC12: number;
	private projC12Abs: number;

	private mDepth: number = MathUtil.POSITIVE_INFINITY;
	private mSign: number = 0;
	private mAxis: Vec3 = new Vec3();

	/**
	 * Default constructor.
	 */
	constructor() {
		super(false);
		this.clipper = new FaceClipper();
	}

	private _SATCheck(): boolean {
		// --------------------- SAT check start ---------------------
		this.mId = -1;
		this.mDepth = Number.MAX_SAFE_INTEGER;
		// --------------------- 6 faces ---------------------

		// try axis = x1
		this.proj1 = this.w1;
		this.proj2 = this.project(this.x1, this.sx2, this.sy2, this.sz2);
		this.projC12 = M.vec3_dot(this.x1, this.pos1_2);
		var calcResult = BoxBoxDetectorMacro.satCheck(this.mDepth, this.mAxis, this.proj1, this.proj2, this.projC12, this.x1, 0);
		if (calcResult && calcResult[0] != 0) {
			this.mDepth = calcResult[0];
			this.mId = calcResult[1];
			this.mSign = calcResult[2];
		} else if (calcResult == null) {
			return false;
		}

		// try axis = y1
		this.proj1 = this.h1;
		this.proj2 = this.project(this.y1, this.sx2, this.sy2, this.sz2);
		this.projC12 = M.vec3_dot(this.y1, this.pos1_2);
		var calcResult = BoxBoxDetectorMacro.satCheck(this.mDepth, this.mAxis, this.proj1, this.proj2, this.projC12, this.y1, 1);
		if (calcResult && calcResult[0] != 0) {
			this.mDepth = calcResult[0];
			this.mId = calcResult[1];
			this.mSign = calcResult[2];
		} else if (calcResult == null) {
			return false;
		}

		// try axis = z1
		this.proj1 = this.d1;
		this.proj2 = this.project(this.z1, this.sx2, this.sy2, this.sz2);
		this.projC12 = M.vec3_dot(this.z1, this.pos1_2);
		var calcResult = BoxBoxDetectorMacro.satCheck(this.mDepth, this.mAxis, this.proj1, this.proj2, this.projC12, this.z1, 2);
		if (calcResult && calcResult[0] != 0) {
			this.mDepth = calcResult[0];
			this.mId = calcResult[1];
			this.mSign = calcResult[2];
		} else if (calcResult == null) {
			return false;
		}

		/*
		// apply bias to avoid jitting
		if (this.mDepth > Setting.linearSlop) {
			this.mDepth -= Setting.linearSlop;
		} else {
			this.mDepth = 0;
		}
		*/

		// try axis = x2
		this.proj1 = this.project(this.x2, this.sx1, this.sy1, this.sz1);
		this.proj2 = this.w2;
		this.projC12 = M.vec3_dot(this.x2, this.pos1_2);
		var calcResult = BoxBoxDetectorMacro.satCheck(this.mDepth, this.mAxis, this.proj1, this.proj2, this.projC12, this.x2, 3);
		if (calcResult && calcResult[0] != 0) {
			this.mDepth = calcResult[0];
			this.mId = calcResult[1];
			this.mSign = calcResult[2];
		} else if (calcResult == null) {
			return false;
		}

		// try axis = y2
		this.proj1 = this.project(this.y2, this.sx1, this.sy1, this.sz1);
		this.proj2 = this.h2;
		this.projC12 = M.vec3_dot(this.y2, this.pos1_2);
		var calcResult = BoxBoxDetectorMacro.satCheck(this.mDepth, this.mAxis, this.proj1, this.proj2, this.projC12, this.y2, 4);
		if (calcResult && calcResult[0] != 0) {
			this.mDepth = calcResult[0];
			this.mId = calcResult[1];
			this.mSign = calcResult[2];
		} else if (calcResult == null) {
			return false;
		}

		// try axis = z2
		this.proj1 = this.project(this.z2, this.sx1, this.sy1, this.sz1);
		this.proj2 = this.d2;
		this.projC12 = M.vec3_dot(this.z2, this.pos1_2);
		var calcResult = BoxBoxDetectorMacro.satCheck(this.mDepth, this.mAxis, this.proj1, this.proj2, this.projC12, this.z2, 5);
		if (calcResult && calcResult[0] != 0) {
			this.mDepth = calcResult[0];
			this.mId = calcResult[1];
			this.mSign = calcResult[2];
		} else if (calcResult == null) {
			return false;
		}

		// --------------------- 9 edges ---------------------

		/*
		// apply bias again to avoid jitting
		if (this.mDepth > Setting.linearSlop) {
			this.mDepth -= Setting.linearSlop;
		} else {
			this.mDepth = 0;
		}
		*/

		var edgeAxis: Vec3 = new Vec3();

		// try cross(x1, x2)
		M.vec3_cross(edgeAxis, this.x1, this.x2);
		if (!M.vec3_isZero(edgeAxis)) {
			edgeAxis.normalize();
			this.proj1 = this.project2(edgeAxis, this.sy1, this.sz1);
			this.proj2 = this.project2(edgeAxis, this.sy2, this.sz2);
			this.projC12 = M.vec3_dot(edgeAxis, this.pos1_2);
			var calcResult = BoxBoxDetectorMacro.satCheck(this.mDepth, this.mAxis, this.proj1, this.proj2, this.projC12, edgeAxis, 6);
			if (calcResult && calcResult[0] != 0) {
				this.mDepth = calcResult[0];
				this.mId = calcResult[1];
				this.mSign = calcResult[2];
			} else if (calcResult == null) {
				return false;
			}
		}

		// try cross(x1, y2)
		M.vec3_cross(edgeAxis, this.x1, this.y2);
		if (!M.vec3_isZero(edgeAxis)) {
			edgeAxis.normalize();
			this.proj1 = this.project2(edgeAxis, this.sy1, this.sz1);
			this.proj2 = this.project2(edgeAxis, this.sx2, this.sz2);
			this.projC12 = M.vec3_dot(edgeAxis, this.pos1_2);
			var calcResult = BoxBoxDetectorMacro.satCheck(this.mDepth, this.mAxis, this.proj1, this.proj2, this.projC12, edgeAxis, 7);
			if (calcResult && calcResult[0] != 0) {
				this.mDepth = calcResult[0];
				this.mId = calcResult[1];
				this.mSign = calcResult[2];
			} else if (calcResult == null) {
				return false;
			}
		}

		// try cross(x1, z2)
		M.vec3_cross(edgeAxis, this.x1, this.z2);
		if (!M.vec3_isZero(edgeAxis)) {
			edgeAxis.normalize();
			this.proj1 = this.project2(edgeAxis, this.sy1, this.sz1);
			this.proj2 = this.project2(edgeAxis, this.sx2, this.sy2);
			this.projC12 = M.vec3_dot(edgeAxis, this.pos1_2);
			var calcResult = BoxBoxDetectorMacro.satCheck(this.mDepth, this.mAxis, this.proj1, this.proj2, this.projC12, edgeAxis, 8);
			if (calcResult && calcResult[0] != 0) {
				this.mDepth = calcResult[0];
				this.mId = calcResult[1];
				this.mSign = calcResult[2];
			} else if (calcResult == null) {
				return false;
			}
		}

		// try cross(y1, x2)
		M.vec3_cross(edgeAxis, this.y1, this.x2);
		if (!M.vec3_isZero(edgeAxis)) {
			edgeAxis.normalize();
			this.proj1 = this.project2(edgeAxis, this.sx1, this.sz1);
			this.proj2 = this.project2(edgeAxis, this.sy2, this.sz2);
			this.projC12 = M.vec3_dot(edgeAxis, this.pos1_2);
			var calcResult = BoxBoxDetectorMacro.satCheck(this.mDepth, this.mAxis, this.proj1, this.proj2, this.projC12, edgeAxis, 9);
			if (calcResult && calcResult[0] != 0) {
				this.mDepth = calcResult[0];
				this.mId = calcResult[1];
				this.mSign = calcResult[2];
			} else if (calcResult == null) {
				return false;
			}
		}

		// try cross(y1, y2)
		M.vec3_cross(edgeAxis, this.y1, this.y2);
		if (!M.vec3_isZero(edgeAxis)) {
			edgeAxis.normalize();
			this.proj1 = this.project2(edgeAxis, this.sx1, this.sz1);
			this.proj2 = this.project2(edgeAxis, this.sx2, this.sz2);
			this.projC12 = M.vec3_dot(edgeAxis, this.pos1_2);
			var calcResult = BoxBoxDetectorMacro.satCheck(this.mDepth, this.mAxis, this.proj1, this.proj2, this.projC12, edgeAxis, 10);
			if (calcResult && calcResult[0] != 0) {
				this.mDepth = calcResult[0];
				this.mId = calcResult[1];
				this.mSign = calcResult[2];
			} else if (calcResult == null) {
				return false;
			}
		}

		// try cross(y1, z2)
		M.vec3_cross(edgeAxis, this.y1, this.z2);
		if (!M.vec3_isZero(edgeAxis)) {
			edgeAxis.normalize();
			this.proj1 = this.project2(edgeAxis, this.sx1, this.sz1);
			this.proj2 = this.project2(edgeAxis, this.sx2, this.sy2);
			this.projC12 = M.vec3_dot(edgeAxis, this.pos1_2);
			var calcResult = BoxBoxDetectorMacro.satCheck(this.mDepth, this.mAxis, this.proj1, this.proj2, this.projC12, edgeAxis, 11);
			if (calcResult && calcResult[0] != 0) {
				this.mDepth = calcResult[0];
				this.mId = calcResult[1];
				this.mSign = calcResult[2];
			} else if (calcResult == null) {
				return false;
			}
		}

		// try cross(z1, x2)
		M.vec3_cross(edgeAxis, this.z1, this.x2);
		if (!M.vec3_isZero(edgeAxis)) {
			edgeAxis.normalize();
			this.proj1 = this.project2(edgeAxis, this.sx1, this.sy1);
			this.proj2 = this.project2(edgeAxis, this.sy2, this.sz2);
			this.projC12 = M.vec3_dot(edgeAxis, this.pos1_2);
			var calcResult = BoxBoxDetectorMacro.satCheck(this.mDepth, this.mAxis, this.proj1, this.proj2, this.projC12, edgeAxis, 12);
			if (calcResult && calcResult[0] != 0) {
				this.mDepth = calcResult[0];
				this.mId = calcResult[1];
				this.mSign = calcResult[2];
			} else if (calcResult == null) {
				return false;
			}
		}

		// try cross(z1, y2)
		M.vec3_cross(edgeAxis, this.z1, this.y2);
		if (!M.vec3_isZero(edgeAxis)) {
			edgeAxis.normalize();
			this.proj1 = this.project2(edgeAxis, this.sx1, this.sy1);
			this.proj2 = this.project2(edgeAxis, this.sx2, this.sz2);
			this.projC12 = M.vec3_dot(edgeAxis, this.pos1_2);
			var calcResult = BoxBoxDetectorMacro.satCheck(this.mDepth, this.mAxis, this.proj1, this.proj2, this.projC12, edgeAxis, 13);
			if (calcResult && calcResult[0] != 0) {
				this.mDepth = calcResult[0];
				this.mId = calcResult[1];
				this.mSign = calcResult[2];
			} else if (calcResult == null) {
				return false;
			}
		}

		// try cross(z1, z2)
		M.vec3_cross(edgeAxis, this.z1, this.z2);
		if (!M.vec3_isZero(edgeAxis)) {
			edgeAxis.normalize();
			this.proj1 = this.project2(edgeAxis, this.sx1, this.sy1);
			this.proj2 = this.project2(edgeAxis, this.sx2, this.sy2);
			this.projC12 = M.vec3_dot(edgeAxis, this.pos1_2);
			var calcResult = BoxBoxDetectorMacro.satCheck(this.mDepth, this.mAxis, this.proj1, this.proj2, this.projC12, edgeAxis, 14);
			if (calcResult && calcResult[0] != 0) {
				this.mDepth = calcResult[0];
				this.mId = calcResult[1];
				this.mSign = calcResult[2];
			} else if (calcResult == null) {
				return false;
			}
		}

		return this.mId != -1;
	}

	public detectImpl(result: DetectorResult, geom1: Geometry, geom2: Geometry, tf1: Transform, tf2: Transform, cachedData: CachedDetectorData): void {
		var b1: BoxGeometry = geom1 as BoxGeometry;
		var b2: BoxGeometry = geom2 as BoxGeometry;

		result.incremental = false;

		// basis of box1 := {x1, y1, z1}
		// basis of box2 := {x2, y2, z2}
		// half-extents of box1 := {w1, h1, d1}
		// half-extents of box2 := {w2, h2, d2}
		//
		// candidates of the separating axis:
		//     x1,
		//     y1,
		//     z1,
		//     x2,
		//     y2,
		//     z2,
		//     cross(x1, x2),
		//     cross(x1, y2),
		//     cross(x1, z2),
		//     cross(y1, x2),
		//     cross(y1, y2),
		//     cross(y1, z2),
		//     cross(z1, x2),
		//     cross(z1, y2),
		//     cross(z1, z2).
		//
		// projected length of box1:
		//   project to       | length
		//   -------------------------------
		//   x1               | w1
		//   y1               | h1
		//   z1               | d1
		//   a = cross(x1, _) | h1|y1.a| + d1|z1.a|
		//   a = cross(y1, _) | w1|x1.a| + d1|z1.a|
		//   a = cross(z1, _) | w1|x1.a| + h1|y1.a|
		//   a = _            | w1|x1.a| + h1|y1.a| + d1|z1.a|
		//
		// projected length of box2:
		//   project to       | length
		//   -------------------------------
		//   x2               | w2
		//   y2               | h2
		//   z2               | d2
		//   a = cross(x2, _) | h2|y2.a| + d2|z2.a|
		//   a = cross(y2, _) | w2|x2.a| + d2|z2.a|
		//   a = cross(z2, _) | w2|x2.a| + h2|y2.a|
		//   a = _            | w2|x2.a| + h2|y2.a| + d2|z2.a|

		this.pos1.copyFrom(tf1._position);
		this.pos2.copyFrom(tf2._position);
		M.vec3_sub(this.pos1_2, this.pos2, this.pos1);

		// bases
		M.mat3_getCol(this.x1, tf1._rotation, 0);
		M.mat3_getCol(this.y1, tf1._rotation, 1);
		M.mat3_getCol(this.z1, tf1._rotation, 2);
		M.mat3_getCol(this.x2, tf2._rotation, 0);
		M.mat3_getCol(this.y2, tf2._rotation, 1);
		M.mat3_getCol(this.z2, tf2._rotation, 2);

		// half extents
		this.w1 = b1._halfExtents.x;
		this.h1 = b1._halfExtents.y;
		this.d1 = b1._halfExtents.z;
		this.w2 = b2._halfExtents.x;
		this.h2 = b2._halfExtents.y;
		this.d2 = b2._halfExtents.z;

		// scaled bases by half extents
		M.vec3_scale(this.sx1, this.x1, this.w1);
		M.vec3_scale(this.sy1, this.y1, this.h1);
		M.vec3_scale(this.sz1, this.z1, this.d1);
		M.vec3_scale(this.sx2, this.x2, this.w2);
		M.vec3_scale(this.sy2, this.y2, this.h2);
		M.vec3_scale(this.sz2, this.z2, this.d2);

		if (!this._SATCheck()) {
			return;
		}

		console.log('碰撞方向', this.mId, this.mSign);


		// --------------------- edge-edge collision check ---------------------

		if (this.mId >= 6) {
			// flip axis so that it directs from box1 to box2
			this._HandleEdgeCollider(result);
			return;
		}

		// --------------------- face-face collision check ---------------------

		var tmp: Vec3 = new Vec3();
		var swapped: boolean;

		var tempNum: any;

		if (this.mId >= 3) { // swap box1 and box2
			this.mSign = -this.mSign;
			this.pos1_2.negateEq();

			tempNum = b1;
			b1 = b2;
			b2 = tempNum;
			// BoxBoxDetectorMacro.swap(b1, b2);

			tempNum = this.w1;
			this.w1 = this.w2;
			this.w2 = tempNum;
			// BoxBoxDetectorMacro.swap(w1, w2);

			tempNum = this.h1;
			this.h1 = this.h2;
			this.h2 = tempNum;
			// BoxBoxDetectorMacro.swap(h1, h2);

			tempNum = this.d1;
			this.d1 = this.d2;
			this.d2 = tempNum;
			// BoxBoxDetectorMacro.swap(d1, d2);
			BoxBoxDetectorMacro.swapV(tmp, this.pos1, this.pos2);
			BoxBoxDetectorMacro.swapV(tmp, this.x1, this.x2);
			BoxBoxDetectorMacro.swapV(tmp, this.y1, this.y2);
			BoxBoxDetectorMacro.swapV(tmp, this.z1, this.z2);
			BoxBoxDetectorMacro.swapV(tmp, this.sx1, this.sx2);
			BoxBoxDetectorMacro.swapV(tmp, this.sy1, this.sy2);
			BoxBoxDetectorMacro.swapV(tmp, this.sz1, this.sz2);

			this.mId -= 3;
			swapped = true;
		} else {
			swapped = false;
		}

		// --------------------- find reference face ---------------------

		var refCenter: Vec3 = new Vec3();
		var refNormal: Vec3 = new Vec3();
		var refX: Vec3 = new Vec3();
		var refY: Vec3 = new Vec3();
		var refW: number;
		var refH: number;

		switch (this.mId) {
			case 0: // x+ or x-
				M.vec3_assign(refCenter, this.sx1);
				M.vec3_assign(refNormal, this.x1);
				M.vec3_assign(refX, this.y1);
				M.vec3_assign(refY, this.z1);
				refW = this.h1;
				refH = this.d1;
				break;
			case 1: // y+ or y-
				M.vec3_assign(refCenter, this.sy1);
				M.vec3_assign(refNormal, this.y1);
				M.vec3_assign(refX, this.z1);
				M.vec3_assign(refY, this.x1);
				refW = this.d1;
				refH = this.w1;
				break;
			default: // z+ or z-
				M.vec3_assign(refCenter, this.sz1);
				M.vec3_assign(refNormal, this.z1);
				M.vec3_assign(refX, this.x1);
				M.vec3_assign(refY, this.y1);
				refW = this.w1;
				refH = this.h1;
				break;
		}

		if (this.mSign < 0) { // x- or y- or z-
			refCenter.negateEq();
			refNormal.negateEq();
			BoxBoxDetectorMacro.swapV(tmp, refX, refY);

			tempNum = refW;
			refW = refH;
			refH = tempNum;
			// BoxBoxDetectorMacro.swap(refW, refH);
		}

		// translate reference center
		M.vec3_add(refCenter, refCenter, this.pos1);

		// --------------------- find incident face ---------------------

		var minIncDot: number = 1;
		var incId: number = 0;

		var incDot: number;
		incDot = M.vec3_dot(refNormal, this.x2);
		if (incDot < minIncDot) { // x+
			minIncDot = incDot;
			incId = 0;
		}
		if (-incDot < minIncDot) { // x-
			minIncDot = -incDot;
			incId = 1;
		}
		incDot = M.vec3_dot(refNormal, this.y2);
		if (incDot < minIncDot) { // y+
			minIncDot = incDot;
			incId = 2;
		}
		if (-incDot < minIncDot) { // y-
			minIncDot = -incDot;
			incId = 3;
		}
		incDot = M.vec3_dot(refNormal, this.z2);
		if (incDot < minIncDot) { // y+
			minIncDot = incDot;
			incId = 4;
		}
		if (-incDot < minIncDot) { // y-
			minIncDot = -incDot;
			incId = 5;
		}

		var incV1: Vec3 = new Vec3();
		var incV2: Vec3 = new Vec3();
		var incV3: Vec3 = new Vec3();
		var incV4: Vec3 = new Vec3();
		BoxBoxDetectorMacro.getBoxFace(incV1, incV2, incV3, incV4, this.sx2, this.sy2, this.sz2, incId);

		M.vec3_add(incV1, incV1, this.pos1_2);
		M.vec3_add(incV2, incV2, this.pos1_2);
		M.vec3_add(incV3, incV3, this.pos1_2);
		M.vec3_add(incV4, incV4, this.pos1_2);

		// --------------------- clip incident face ---------------------

		this.clipper.init(refW, refH);
		this.clipper.addIncidentVertex(M.vec3_dot(incV1, refX), M.vec3_dot(incV1, refY), M.vec3_get(incV1, 0), M.vec3_get(incV1, 1), M.vec3_get(incV1, 2));
		this.clipper.addIncidentVertex(M.vec3_dot(incV2, refX), M.vec3_dot(incV2, refY), M.vec3_get(incV2, 0), M.vec3_get(incV2, 1), M.vec3_get(incV2, 2));
		this.clipper.addIncidentVertex(M.vec3_dot(incV3, refX), M.vec3_dot(incV3, refY), M.vec3_get(incV3, 0), M.vec3_get(incV3, 1), M.vec3_get(incV3, 2));
		this.clipper.addIncidentVertex(M.vec3_dot(incV4, refX), M.vec3_dot(incV4, refY), M.vec3_get(incV4, 0), M.vec3_get(incV4, 1), M.vec3_get(incV4, 2));
		this.clipper.clip();

		// --------------------- reduce vertices ---------------------

		this.clipper.reduce();

		// --------------------- add contact points ---------------------

		// set normal
		var normal: Vec3 = new Vec3();
		if (swapped) {
			normal = refNormal.clone();
		} else {
			normal = refNormal.negate();
		}
		this.setNormal(result, normal);

		// add contact points
		for (let i = 0; i < this.clipper.numVertices; ++i) {
			var v: IncidentVertex = this.clipper.vertices[i];

			var clippedVertex: Vec3 = new Vec3(v.wx, v.wy, v.wz);
			M.vec3_add(clippedVertex, clippedVertex, this.pos1);

			var clippedVertexToRefCenter: Vec3 = new Vec3();
			M.vec3_sub(clippedVertexToRefCenter, refCenter, clippedVertex);
			var depth: number = M.vec3_dot(clippedVertexToRefCenter, refNormal);

			var clippedVertexOnRefFace: Vec3 = new Vec3();
			M.vec3_addRhsScaledTo(clippedVertexOnRefFace, clippedVertex, refNormal, depth);

			if (depth > -Setting.contactPersistenceThreshold) {
				if (swapped) {
					this.addPoint(result, clippedVertex, clippedVertexOnRefFace, depth, i);
				} else {
					this.addPoint(result, clippedVertexOnRefFace, clippedVertex, depth, i);
				}
			}
		}
	}

	private _HandleEdgeCollider(result: DetectorResult) {
		M.vec3_scale(this.mAxis, this.mAxis, this.mSign);

		// direction of edges: 0 = x, 1 = y, 2 = z
		var id1: number = Math.round((this.mId - 6) / 3);
		var id2: number = (this.mId - 6) - id1 * 3;

		// points on the edges
		var p1: Vec3 = new Vec3();
		var p2: Vec3 = new Vec3();

		// directions
		let d1: Vec3 = new Vec3();
		let d2: Vec3 = new Vec3();

		switch (id1) {
			case 0: // use y and z
				M.vec3_assign(d1, this.x1);
				BoxBoxDetectorMacro.supportingVertexRect(p1, this.sy1, this.sz1, this.mAxis);
				break;
			case 1: // use x and z
				M.vec3_assign(d1, this.y1);
				BoxBoxDetectorMacro.supportingVertexRect(p1, this.sx1, this.sz1, this.mAxis);
				break;
			default: // use x and y
				M.vec3_assign(d1, this.z1);
				BoxBoxDetectorMacro.supportingVertexRect(p1, this.sx1, this.sy1, this.mAxis);
				break;
		}
		M.vec3_add(p1, this.pos1, p1);

		switch (id2) {
			case 0: // use y and z
				M.vec3_assign(d2, this.x2);
				BoxBoxDetectorMacro.supportingVertexRect(p2, this.sy2, this.sz2, this.mAxis);
				break;
			case 1: // use x and z
				M.vec3_assign(d2, this.y2);
				BoxBoxDetectorMacro.supportingVertexRect(p2, this.sx2, this.sz2, this.mAxis);
				break;
			default: // use x and y
				M.vec3_assign(d2, this.z2);
				BoxBoxDetectorMacro.supportingVertexRect(p2, this.sx2, this.sy2, this.mAxis);
				break;
		}
		M.vec3_sub(p2, this.pos2, p2);

		// compute params
		var r: Vec3 = new Vec3();
		M.vec3_sub(r, p1, p2);

		var dot12: number = M.vec3_dot(d1, d2);
		var dot1r: number = M.vec3_dot(d1, r);
		var dot2r: number = M.vec3_dot(d2, r);

		var invDet: number = 1 / (1 - dot12 * dot12);
		var t1: number = (dot12 * dot2r - dot1r) * invDet;
		var t2: number = (dot2r - dot12 * dot1r) * invDet;

		// compute closest points and normal
		var cp1: Vec3 = new Vec3();
		var cp2: Vec3 = new Vec3();
		M.vec3_addRhsScaledTo(cp1, p1, d1, t1);
		M.vec3_addRhsScaledTo(cp2, p2, d2, t2);

		var normal: Vec3 = this.mAxis.normalized();

		// add contact point
		this.setNormal(result, normal);
		this.addPoint(result, cp1, cp2, this.mDepth, 4);
	}

	/**
	 * Returns half of the projected length of the box with scaled bases
	 * (`sx`, `sy`, `sz`) onto the normalized axis `axis`.
	 */

	private project(axis: Vec3, sx: Vec3, sy: Vec3, sz: Vec3): number {
		var dx: number = M.vec3_dot(axis, sx);
		var dy: number = M.vec3_dot(axis, sy);
		var dz: number = M.vec3_dot(axis, sz);
		if (dx < 0) dx = -dx;
		if (dy < 0) dy = -dy;
		if (dz < 0) dz = -dz;
		return dx + dy + dz;
	}

	/**
	 * 2D version of `project`.
	 */

	private project2(axis: Vec3, sx: Vec3, sy: Vec3): number {
		var dx: number = M.vec3_dot(axis, sx);
		var dy: number = M.vec3_dot(axis, sy);
		if (dx < 0) dx = -dx;
		if (dy < 0) dy = -dy;
		return dx + dy;
	}
}

class IncidentVertex {
	// projected coord
	x: number;
	y: number;

	// world coord
	wx: number;
	wy: number;
	wz: number;

	constructor() {
		this.x = 0;
		this.y = 0;
		this.wx = 0;
		this.wy = 0;
		this.wz = 0;
	}


	public init(x: number, y: number, wx: number, wy: number, wz: number): void {
		this.x = x;
		this.y = y;
		this.wx = wx;
		this.wy = wy;
		this.wz = wz;
	}


	public copyFrom(v: IncidentVertex): void {
		this.x = v.x;
		this.y = v.y;
		this.wx = v.wx;
		this.wy = v.wy;
		this.wz = v.wz;
	}


	public interp(v1: IncidentVertex, v2: IncidentVertex, t: number): void {
		this.x = v1.x + (v2.x - v1.x) * t;
		this.y = v1.y + (v2.y - v1.y) * t;
		this.wx = v1.wx + (v2.wx - v1.wx) * t;
		this.wy = v1.wy + (v2.wy - v1.wy) * t;
		this.wz = v1.wz + (v2.wz - v1.wz) * t;
	}
}

class FaceClipper {
	w: number;
	h: number;
	numVertices: number;
	vertices: Array<IncidentVertex>;
	numTmpVertices: number;
	tmpVertices: Array<IncidentVertex>;

	constructor() {
		this.w = 0;
		this.h = 0;
		this.numVertices = 0;
		this.numTmpVertices = 0;
		this.vertices = new Array<IncidentVertex>(8);
		this.tmpVertices = new Array<IncidentVertex>(8);
		for (let i = 0; i < 8; ++i) {
			this.vertices[i] = new IncidentVertex();
			this.tmpVertices[i] = new IncidentVertex();
		}
	}


	public init(w: number, h: number): void {
		this.w = w;
		this.h = h;
		this.numVertices = 0;
		this.numTmpVertices = 0;
	}


	public addIncidentVertex(x: number, y: number, wx: number, wy: number, wz: number): void {
		this.vertices[this.numVertices++].init(x, y, wx, wy, wz);
	}

	/**
	 * Clips the incident face by the reference face, generates up to eight vertices.
	 */
	public clip(): void {
		this.clipL();
		this.flip();
		this.clipR();
		this.flip();
		this.clipT();
		this.flip();
		this.clipB();
		this.flip();
	}

	/**
	 * Reduces vertices up to four.
	 */
	public reduce(): void {
		if (this.numVertices < 4) return;

		// TODO: maximize area
		var max1: number = MathUtil.NEGATIVE_INFINITY;
		var min1: number = MathUtil.POSITIVE_INFINITY;
		var max2: number = MathUtil.NEGATIVE_INFINITY;
		var min2: number = MathUtil.POSITIVE_INFINITY;
		var max1V: IncidentVertex = null;
		var min1V: IncidentVertex = null;
		var max2V: IncidentVertex = null;
		var min2V: IncidentVertex = null;
		var e1x: number = 1;
		var e1y: number = 1;
		var e2x: number = -1;
		var e2y: number = 1;

		for (let i = 0; i < this.numVertices; ++i) {
			var v: IncidentVertex = this.vertices[i];
			var dot1: number = v.x * e1x + v.y * e1y;
			var dot2: number = v.x * e2x + v.y * e2y;
			if (dot1 > max1) {
				max1 = dot1;
				max1V = v;
			}
			if (dot1 < min1) {
				min1 = dot1;
				min1V = v;
			}
			if (dot2 > max2) {
				max2 = dot2;
				max2V = v;
			}
			if (dot2 < min2) {
				min2 = dot2;
				min2V = v;
			}
		}

		this.add(max1V);
		this.add(max2V);
		this.add(min1V);
		this.add(min2V);
		this.flip();
	}


	private clipL(): void {
		for (let i = 0; i < this.numVertices; ++i) {
			var v1: IncidentVertex = this.vertices[i];
			var v2: IncidentVertex = this.vertices[(i + 1) % this.numVertices];
			var s1: number = this.w + v1.x;
			var s2: number = this.w + v2.x;
			this.clipWithParam(v1, v2, s1, s2);
		}
	}


	private clipR(): void {
		for (let i = 0; i < this.numVertices; ++i) {
			var v1: IncidentVertex = this.vertices[i];
			var v2: IncidentVertex = this.vertices[(i + 1) % this.numVertices];
			var s1: number = this.w - v1.x;
			var s2: number = this.w - v2.x;
			this.clipWithParam(v1, v2, s1, s2);
		}
	}


	private clipT(): void {
		for (let i = 0; i < this.numVertices; ++i) {
			var v1: IncidentVertex = this.vertices[i];
			var v2: IncidentVertex = this.vertices[(i + 1) % this.numVertices];
			var s1: number = this.h + v1.y;
			var s2: number = this.h + v2.y;
			this.clipWithParam(v1, v2, s1, s2);
		}
	}


	private clipB(): void {
		for (let i = 0; i < this.numVertices; ++i) {
			var v1: IncidentVertex = this.vertices[i];
			var v2: IncidentVertex = this.vertices[(i + 1) % this.numVertices];
			var s1: number = this.h - v1.y;
			var s2: number = this.h - v2.y;
			this.clipWithParam(v1, v2, s1, s2);
		}
	}


	private flip(): void {
		let cache = this.vertices;
		this.vertices = this.tmpVertices;
		this.tmpVertices = cache;
		this.numVertices = this.numTmpVertices;
		this.numTmpVertices = 0;
	}


	private clipWithParam(v1: IncidentVertex, v2: IncidentVertex, s1: number, s2: number): void {
		if (s1 > 0 && s2 > 0) {
			this.add(v1);
		} else if (s1 > 0 && s2 <= 0) {
			// v2 is clipped
			this.add(v1);
			this.interp(v1, v2, s1 / (s1 - s2));
		} else if (s1 <= 0 && s2 > 0) {
			// v1 is clipped
			this.interp(v1, v2, s1 / (s1 - s2));
		}
	}


	private add(v: IncidentVertex): void {
		this.tmpVertices[this.numTmpVertices++].copyFrom(v);
	}


	private interp(v1: IncidentVertex, v2: IncidentVertex, t: number): void {
		this.tmpVertices[this.numTmpVertices++].interp(v1, v2, t);
	}
}
